#if !defined(MLP_INPUT_H_)
#define MLP_INPUT_H_
#include <exception>
#include <string>
#include <vector>

#include "common/utility.h"

namespace autodrive {
namespace perception {
struct MLPInputParameters {
  /// @brief camera list
  std::vector<std::string> camera_list;
  /// @brief post rotation  0 list
  std::vector<float> post_rot_0_list;
  /// @brief post rotation 1 list
  std::vector<float> post_rot_1_list;
  /// @brief post transplate 0 list
  std::vector<float> post_trans_0_list;
  /// @brief post transplate 1 list
  std::vector<float> post_trans_1_list;
};

struct MLPInputParameter {
  /// @brief intrinsic
  std::vector<double> intrinsic;
  /// @brief  extrinsic
  std::vector<double> extrinsic;
  /// @brief distortion
  std::vector<double> distortion;
  /// @brief post rotate 0
  double post_rot_0;
  /// @brief post roate 1
  double post_rot_1;
  /// @brief post trans 0
  double post_tran_0;
  /// @brief post trans 1
  double post_tran_1;
};

template <typename T>
void gen_mlp_input(MLPInputParameter const& para, std::int16_t scale,
                   std::vector<T>& out) noexcept(false) {
  constexpr std::int32_t intrinsicSize{3 * 3};
  constexpr std::int32_t extrinsicSize{4 * 4};
  constexpr std::int32_t mlpDataSize{37};
  constexpr float quantifyOffset{0.5};

  auto const& intrinsic{para.intrinsic};
  auto const& extrinsic{para.extrinsic};
  auto const& distortion{para.distortion};
  auto const& post_rot_0{para.post_rot_0};
  auto const& post_rot_1{para.post_rot_1};
  auto const& post_tran_0{para.post_tran_0};
  auto const& post_tran_1{para.post_tran_1};

  if (intrinsic.size() != intrinsicSize) {
    throw std::runtime_error(
        "intrinsic size()=" + std::to_string(intrinsic.size()) + " is not " +
        std::to_string(intrinsicSize));
  }

  if (extrinsic.size() != extrinsicSize) {
    throw std::runtime_error(
        "extrinsic size()=" + std::to_string(extrinsic.size()) + " is not" +
        std::to_string(extrinsicSize));
  }

  out.clear();
  out.reserve(mlpDataSize);

  // intrinsic
  out.emplace_back(quantify<double, T>(intrinsic.at(0), scale, quantifyOffset));
  out.emplace_back(quantify<double, T>(intrinsic.at(4), scale, quantifyOffset));
  out.emplace_back(quantify<double, T>(intrinsic.at(2), scale, quantifyOffset));
  out.emplace_back(quantify<double, T>(intrinsic.at(5), scale, quantifyOffset));

  // post rot 0
  out.emplace_back(quantify<double, T>(post_rot_0, scale, quantifyOffset));
  out.emplace_back(quantify<double, T>(0, scale, quantifyOffset));

  // post trans 0
  out.emplace_back(quantify<double, T>(post_tran_0, scale, quantifyOffset));
  out.emplace_back(quantify<double, T>(0, scale, quantifyOffset));

  // post rot1
  out.emplace_back(quantify<double, T>(post_rot_1, scale, quantifyOffset));
  // post trans1
  out.emplace_back(quantify<double, T>(post_tran_1, scale, quantifyOffset));

  // bda
  out.emplace_back(quantify<double, T>(1, scale, quantifyOffset));
  out.emplace_back(quantify<double, T>(0, scale, quantifyOffset));
  out.emplace_back(quantify<double, T>(0, scale, quantifyOffset));
  out.emplace_back(quantify<double, T>(1, scale, quantifyOffset));
  out.emplace_back(quantify<double, T>(1, scale, quantifyOffset));

  // extrinsic
  constexpr std::int32_t extrinsicQuantifySize{12};
  for (std::int32_t i{0}; i < extrinsicQuantifySize; i++) {
    out.emplace_back(
        quantify<double, T>(extrinsic.at(i), scale, quantifyOffset));
  }

  // distortion
  constexpr std::int32_t distortionDataSize{9};
  if (!distortion.empty() && (distortion.size() < distortionDataSize)) {
    throw std::runtime_error(
        "distortion size()=" + std::to_string(distortion.size()) +
        " is not equal to " + std::to_string(distortionDataSize));
  }
  if (distortion.empty()) {
    for (std::int32_t i{0}; i <= distortionDataSize; ++i) {
      out.emplace_back(quantify<double, T>(0, scale, quantifyOffset));
    }
  } else {
    out.emplace_back(quantify<double, T>(1, scale, quantifyOffset));
    for (std::int32_t i{0}; i < distortionDataSize; ++i) {
      out.emplace_back(
          quantify<double, T>(distortion.at(i), scale, quantifyOffset));
    }
  }
}

template <typename T>
void gen_mlp_input_stack(
    MLPInputParameters const& para,
    std::vector<std::vector<double>> const& intrinsic_list,
    std::vector<std::vector<double>> const& extrinsic_list,
    std::vector<std::vector<double>> const& distortion_list, std::int16_t scale,
    std::vector<std::vector<T>>& out) noexcept(false) {
  auto const& cameraList{para.camera_list};
  auto const& cameraNumber{cameraList.size()};

  if (cameraNumber != intrinsic_list.size()) {
    throw std::runtime_error("camera number=" + std::to_string(cameraNumber) +
                             " is not equal to intrinsic_list number=" +
                             std::to_string(intrinsic_list.size()));
  }

  if (cameraNumber != extrinsic_list.size()) {
    throw std::runtime_error("camera number=" + std::to_string(cameraNumber) +
                             " is not equal to extrinsic_list number=" +
                             std::to_string(extrinsic_list.size()));
  }

  if (cameraNumber != distortion_list.size()) {
    throw std::runtime_error("camera number=" + std::to_string(cameraNumber) +
                             " is not equal to distortion_list number=" +
                             std::to_string(distortion_list.size()));
  }

  auto const& postRot0List{para.post_rot_0_list};
  auto const& postRot1List{para.post_rot_1_list};

  auto const& postTrans0List{para.post_trans_0_list};
  auto const& postTrans1List{para.post_trans_1_list};

  if (cameraNumber != postRot0List.size()) {
    throw std::runtime_error("camera number=" + std::to_string(cameraNumber) +
                             " is not equal to postRot0List number=" +
                             std::to_string(postRot0List.size()));
  }

  if (cameraNumber != postRot1List.size()) {
    throw std::runtime_error("camera number=" + std::to_string(cameraNumber) +
                             " is not equal to postRot1List number=" +
                             std::to_string(postRot1List.size()));
  }

  if (cameraNumber != postTrans0List.size()) {
    throw std::runtime_error("camera number=" + std::to_string(cameraNumber) +
                             " is not equal to postTrans0List number=" +
                             std::to_string(postTrans0List.size()));
  }

  if (cameraNumber != postTrans1List.size()) {
    throw std::runtime_error("camera number=" + std::to_string(cameraNumber) +
                             " is not equal to postTrans1List number=" +
                             std::to_string(postTrans1List.size()));
  }

  out.clear();
  for (std::int32_t i{0}; i < cameraNumber; ++i) {
    std::vector<T> input;
    try {
      MLPInputParameter mlpPara{.intrinsic{intrinsic_list.at(i)},
                                .extrinsic{extrinsic_list.at(i)},
                                .distortion{distortion_list.at(i)},
                                .post_rot_0{postRot0List.at(i)},
                                .post_rot_1{postRot1List.at(i)},
                                .post_tran_0{postTrans0List.at(i)},
                                .post_tran_1{postTrans1List.at(i)}};

      gen_mlp_input(mlpPara, scale, input);
    } catch (const std::exception& e) {
      throw std::runtime_error("camera " + cameraList.at(i) +
                               " gen mlp input error, " + e.what());
    }

    out.emplace_back(input);
  }
}

}  // namespace perception
}  // namespace autodrive

#endif  // MLP_INPUT_H_
